//=============================================================================
//     _   _                               ____                _  _          
//    | | | |  __ _  _ __   _ __   _   _  / ___|  _ __   _ __ (_)| |_   ___ 
//    | |_| | / _` || '_ \ | '_ \ | | | | \___ \ | '_ \ | '__|| || __| / _ \
//    |  _  || (_| || |_) || |_) || |_| |  ___) || |_) || |   | || |_ |  __/
//    |_| |_| \__,_|| .__/ | .__/  \__, | |____/ | .__/ |_|   |_| \__| \___|
//                  |_|    |_|     |___/         |_|                         
//
//                     HappySprite - We make sprites happy
//
// Copyright (c) 2007 by Tank Monkey Games
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//=============================================================================

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite.Simulation
{
    internal class GjkSimplexSolver
    {
        static int[] bitsToIndices =
            {
                0,  // {}
                1,  // {1}
                2,  // {2}
                9,  // {2, 1}
                3,  // {3}
                13, // {3, 1}
                14, // {3, 2}
                57  // {3, 2, 1}
            };

        Vector2[][] edges = new Vector2[][] { new Vector2[3], new Vector2[3], new Vector2[3] };
        float[][] edgeLengthSq = new float[][] { new float[3], new float[3], new float[3] };
        Vector2[] y = new Vector2[3];
        Vector2[] p = new Vector2[3];
        Vector2[] q = new Vector2[3];
        float[] yLengthSq = new float[3];
        float[][] det;

        int simplexBits;
        float maxLengthSq;
        Vector2 closestPoint;

        internal GjkSimplexSolver()
        {
            det = new float[8][];

            for (int i = 0; i < 8; i++)
                det[i] = new float[3];
        }

        internal Vector2 ClosestPoint
        {
            get { return closestPoint; }
        }

        internal bool FullSimplex
        {
            get { return (simplexBits == 7); }
        }

        internal float MaxLengthSquared
        {
            get { return maxLengthSq; }
        }

        internal bool AddSupportPoint(Vector2 newPoint, Vector2 pSupport, Vector2 qSupport)
        {
            int newIndex = (bitsToIndices[simplexBits ^ 7] & 3) - 1;
            y[newIndex] = newPoint;
            p[newIndex] = pSupport;
            q[newIndex] = qSupport;
            yLengthSq[newIndex] = newPoint.LengthSquared();

            for (int i = bitsToIndices[simplexBits]; i != 0; i = i >> 2)
            {
                int index = (i & 3) - 1;
                Vector2 v = y[index] - newPoint;
                edges[index][newIndex] = v;
                edges[newIndex][index] = -v;
                edgeLengthSq[newIndex][index] = edgeLengthSq[index][newIndex] = v.LengthSquared();
            }

            UpdateDeterminant(newIndex);
            return UpdateSimplex(newIndex);
        }

        internal void ComputeClosestPoints(out Vector2 pSupport, out Vector2 qSupport)
        {
            float d = 0f;
            pSupport = Vector2.Zero;
            qSupport = Vector2.Zero;

            for (int i = bitsToIndices[simplexBits]; i != 0; i = i >> 2)
            {
                int index = (i & 3) - 1;
                float delta = det[simplexBits][index];
                d += delta;
                pSupport += p[index] * delta;
                qSupport += q[index] * delta;
            }

            pSupport /= d;
            qSupport /= d;
        }

        internal void Reset()
        {
            simplexBits = 0;
            maxLengthSq = 0f;
        }

        internal IEnumerable<Vector2> SimplexPoints()
        {
            for (int i = bitsToIndices[simplexBits]; i != 0; i = i >> 2)
            {
                int index = (i & 3) - 1;
                yield return y[index];
            }
        }

        Vector2 ComputeClosestPoint()
        {
            float d = 0f;
            Vector2 v = Vector2.Zero;
            maxLengthSq = 0f;

            for (int i = bitsToIndices[simplexBits]; i != 0; i = i >> 2)
            {
                int index = (i & 3) - 1;
                float delta = det[simplexBits][index];
                d += delta;
                v += y[index] * delta;
                maxLengthSq = MathHelper.Max(maxLengthSq, yLengthSq[index]);
            }

            return v / d;
        }

        void UpdateDeterminant(int xmIdx)
        {
            int next = 1 << xmIdx;
            det[next][xmIdx] = 1f;

            int max = bitsToIndices[simplexBits];
            int bi = max;

            for (int i = 0; bi != 0; i++)
            {
                int ii = (bi & 3) - 1;
                int ni = 1 << ii;
                int di = ni | next;

                det[di][ii] = Vector2.Dot(edges[xmIdx][ii], y[xmIdx]);
                det[di][xmIdx] = Vector2.Dot(edges[ii][xmIdx], y[ii]);

                int bj = max;

                for (int j = 0; j < i; j++)
                {
                    int ij = (bj & 3) - 1;
                    int nj = 1 << ij;
                    int dj = di | nj;

                    int idx = (edgeLengthSq[ii][ij] < edgeLengthSq[xmIdx][ij]) ? ii : xmIdx;
                    det[dj][ij] = (det[di][ii] * Vector2.Dot(edges[idx][ij], y[ii])) + (det[di][xmIdx] * Vector2.Dot(edges[idx][ij], y[xmIdx]));

                    idx = (edgeLengthSq[ij][ii] < edgeLengthSq[xmIdx][ii]) ? ij : xmIdx;
                    det[dj][ii] = (det[nj | next][ij] * Vector2.Dot(edges[idx][ii], y[ij])) + (det[nj | next][xmIdx] * Vector2.Dot(edges[idx][ii], y[xmIdx]));

                    idx = (edgeLengthSq[ii][xmIdx] < edgeLengthSq[ij][xmIdx]) ? ii : ij;
                    det[dj][xmIdx] = (det[ni | nj][ij] * Vector2.Dot(edges[idx][xmIdx], y[ij])) + (det[ni | nj][ii] * Vector2.Dot(edges[idx][xmIdx], y[ii]));

                    bj = bj >> 2;
                }

                bi = bi >> 2;
            }

            if ((simplexBits | next) == 7)
            {
                int idx = (edgeLengthSq[1][0] < edgeLengthSq[2][0]) ? 1 : 2;
                det[7][0] = ((det[6][1] * Vector2.Dot(edges[idx][0], y[1])) + (det[6][2] * Vector2.Dot(edges[idx][0], y[2])));

                idx = (edgeLengthSq[0][1] < edgeLengthSq[2][1]) ? 0 : 2;
                det[7][1] = ((det[5][0] * Vector2.Dot(edges[idx][1], y[0])) + (det[5][2] * Vector2.Dot(edges[idx][1], y[2])));

                idx = (edgeLengthSq[0][2] < edgeLengthSq[1][2]) ? 0 : 1;
                det[7][2] = ((det[3][0] * Vector2.Dot(edges[idx][2], y[0])) + (det[3][1] * Vector2.Dot(edges[idx][2], y[1])));
            }
        }

        bool UpdateSimplex(int newIndex)
        {
            int yBits = simplexBits | (1 << newIndex);
            int xBits = 1 << newIndex;

            for (int i = simplexBits; i != 0; i--)
            {
                if (((i & yBits) == i) && IsSatisfiesRule(i | xBits, yBits))
                {
                    simplexBits = i | xBits;
                    closestPoint = ComputeClosestPoint();
                    return true;
                }
            }

            if (IsSatisfiesRule(xBits, yBits))
            {
                simplexBits = xBits;
                closestPoint = y[newIndex];
                maxLengthSq = yLengthSq[newIndex];
                return true;
            }

            return false;
        }

        bool IsSatisfiesRule(int xBits, int yBits)
        {
            for (int i = bitsToIndices[yBits]; i != 0; i = i >> 2)
            {
                int x = (i & 3) - 1;
                int y = 1 << x;

                if ((y & xBits) != 0)
                {
                    if (det[xBits][x] <= 0f)
                        return false;
                }
                else if (det[xBits | y][x] > 0f)
                    return false;
            }

            return true;
        }
    }
}
